#include "entity3d.h"
#include "render.h"
#include "draw_utils.h"

void TEntity3D::render( ) { 
	Globals::g_pd3dDevice->SetTransform( D3DTS_WORLD, &world );
	drawAxis();
}

void TEntity3D::setWorld( const D3DXMATRIX &new_world ) { 
	world = new_world; 

	// En teoria mi matrix no tiene factor de scala.
	D3DXVECTOR3 front = getFront( );
	//assert( fabsf( D3DXVec3Length( &front ) - 1.0f ) <= 1e-6 );
	float err = D3DXVec3Length( &front );
	assert( fabsf( err - 1.0f ) <= 1e-5f );
}

void TEntity3D::setPosition( const D3DXVECTOR3 &new_pos ) {
	world.m[3][0] = new_pos.x;
	world.m[3][1] = new_pos.y;
	world.m[3][2] = new_pos.z;
}

void TEntity3D::move( const D3DXVECTOR3 &delta ) {
	world.m[3][0] += delta.x;
	world.m[3][1] += delta.y;
	world.m[3][2] += delta.z;
}

void TEntity3D::rotate( const D3DXVECTOR3& axis, float radians ) {
	D3DXMATRIX delta;
	D3DXMatrixRotationAxis( &delta, &axis, radians );
	// p' = plocal * world * view * proj
	setWorld( delta * world );
}

void TEntity3D::rotateY( float radians ) {
	rotate( D3DXVECTOR3(0,1,0), radians );
}

const D3DXVECTOR3 &TEntity3D::getPosition() const {
	return *(D3DXVECTOR3*)(&world.m[3][0]);
}

const D3DXVECTOR3 &TEntity3D::getFront( ) const {
	return *(D3DXVECTOR3*)(&world.m[2][0]);
}

const D3DXVECTOR3 &TEntity3D::getLeft( ) const {
	return *(D3DXVECTOR3*)(&world.m[0][0]);
}

const D3DXVECTOR3 &TEntity3D::getUp( ) const {
	return *(D3DXVECTOR3*)(&world.m[1][0]);
}

bool TEntity3D::isInsideVisionCone( const D3DXVECTOR3& loc, float half_fov,  const D3DXVECTOR3& loc_or,float zfar ) const {
	float angle = fabsf( getAngleToPoint( loc ) );
	D3DXVECTOR3 distVector = loc - loc_or;
	float dist = distVector.x * distVector.x + distVector.y*distVector.y + distVector.z * distVector.z;


	return angle < half_fov && (zfar*zfar >= dist);
}

float TEntity3D::calDist(D3DXVECTOR3& loc_or, D3DXVECTOR3& loc_dest) {
	D3DXVECTOR3 distVector = loc_dest - loc_or;
	float dist = distVector.x * distVector.x + distVector.y*distVector.y + distVector.z * distVector.z;
	return dist;
}

// Angulo con signo, asumiendo que giro sobre el eje Y
float TEntity3D::getAngleToPoint( const D3DXVECTOR3& loc ) const {
	D3DXVECTOR3 delta = loc - getPosition();

	// Convert loc to local space
	float proj_in_front = D3DXVec3Dot( &getFront(), &delta );
	float proj_in_left  = D3DXVec3Dot( &getLeft(), &delta );

	// use atan2f, never acosf!!
	float angle = atan2f( proj_in_left, proj_in_front );
	return angle;
}

float TEntity3D::getDistanceTo( const D3DXVECTOR3& loc ) const {
	D3DXVECTOR3 delta = getPosition() - loc;
	return D3DXVec3Length( &delta );
}

// ---------------------------
TEntity3D::TEntity3D( ) {
	D3DXMatrixIdentity( &world );
}

